#include <string.h>

#include "../pkt_buf.h"
#include "../debug.h"

inline void pkt_init(struct pkt* p)
{
    INIT_LIST_HEAD(&p->hdr.list);
    p->hdr.head = p->body;
    p->hdr.tail = p->body + PKT_BODYLEN;
    p->hdr.data = p->hdr.data_end = p->body;
    p->hdr.data_len = 0;
    p->hdr.mac_header = p->hdr.network_header = p->hdr.transport_header = p->hdr.data;
}

static inline void pkt_over_panic(struct pkt* p, int sz, void* here)
{
    ERROR(1, ("pkt_over_panic: text:%p len:%d put:%d "
          "head:%p data:%p data_end:%#lx tail:%#lx\n",
          here, p->hdr.data_len, sz,
          p->hdr.head, p->hdr.data, (unsigned long)p->hdr.data_end, (unsigned long)p->hdr.tail));
    ASSERT(1, ("pkt over panic"));
}

static inline void pkt_under_panic(struct pkt* p, int sz, void* here)
{
    ERROR(1, ("pkt_under_panic: text:%p len:%d put:%d "
          "head:%p data:%p data_end:%#lx tail:%#lx\n",
          here, p->hdr.data_len, sz,
          p->hdr.head, p->hdr.data, (unsigned long)p->hdr.data_end, (unsigned long)p->hdr.tail)
         );
    ASSERT(1, ("pkt under panic"));
}

inline uint8_t* pkt_put(struct pkt* p, unsigned int len)
{
    uint8_t* tmp = p->hdr.data_end;
    p->hdr.data_end += len;
    if (p->hdr.data_end >= p->hdr.tail)
    {
        pkt_over_panic(p, len, __builtin_return_address(0));
    }
    return tmp;
}

static inline uint8_t* pkt_push(struct pkt* p, unsigned int len)
{
    p->hdr.data -= len;
    if (p->hdr.data < p->hdr.head)
    {
        pkt_under_panic(p, len, __builtin_return_address(0));
    }
    return p->hdr.data;
}

inline uint8_t* pkt_pull(struct pkt* p, unsigned int len)
{
    if ((p->hdr.data + len) >= p->hdr.data_end)
    {
        return 0;
    }
    else
    {
        return p->hdr.data += len;
    }
}

inline uint8_t* pkt_pick_data(struct pkt* p, unsigned int len)
{
    return pkt_pull(p, len);
}

uint8_t* pkt_pick_u8(struct pkt* p, uint8_t* to)
{
    uint8_t* p0 = pkt_pull(p, 1);

    if (p0)
    {
        *to = *p0;
    }

    return p0;
}
uint16_t* pkt_pick_u16(struct pkt* p, uint16_t* to)
{
    uint16_t* p0 = (uint16_t*)pkt_pull(p, 2);

    if (p0)
    {
        *to = *p0;
    }

    return p0;
}

uint32_t* pkt_pick_u32(struct pkt* p, uint32_t* to)
{
    uint32_t* p0 = (uint32_t*)pkt_pull(p, 4);

    if (p0)
    {
        *to = *p0;
    }

    return p0;
}

inline void pkt_append_data(struct pkt* p, uint8_t* from, unsigned int copy)
{
    uint8_t* d = pkt_put(p, copy);
    memcpy(d, from, copy);
}

inline void pkt_append_u8(struct pkt* p, uint8_t from)
{
    uint8_t* d = pkt_put(p, 1);
    d[0] = from;
}

inline void pkt_append_u16(struct pkt* p, uint16_t from)
{
    pkt_append_u8(p, (from >> 8) & 0xff);
    pkt_append_u8(p, from & 0xff);
}

inline void pkt_append_u32(struct pkt* p, uint32_t from)
{
    pkt_append_u16(p, (from >> 16) & 0xffff);
    pkt_append_u16(p, from & 0xffff);
}


inline void pkt_prepend_data(struct pkt* p, uint8_t* from, unsigned int copy)
{
    uint8_t* d = pkt_push(p, copy);
    memcpy(d, from, copy);
}


inline void pkt_prepend_u8(struct pkt* p, uint8_t from)
{
    uint8_t* d = pkt_push(p, 1);
    d[0] = from;
}

inline void pkt_prepend_u16(struct pkt* p, uint16_t from)
{
    pkt_prepend_u8(p, (from >> 8) & 0xff);
    pkt_prepend_u8(p, (from) & 0xff);
}

inline void pkt_prepend_u32(struct pkt* p, uint32_t from)
{
    pkt_prepend_u16(p, (from >> 16) & 0xffff);
    pkt_prepend_u16(p, (from) & 0xffff);
}


inline void pkt_reserve(struct pkt* p, int len)
{
    p->hdr.data += len;
    p->hdr.data_end += len;
}

#if 0
void pkt_recv(struct pkt* p)
{
    pkt_put(p, netio_recvpkt(p->body, PKT_BODYLEN));
}

void pkt_send(struct pkt* p)
{
    pkt_pull(p, netio_sendpkt(p->hdr.data, p->hdr.data_end - p->hdr.data));
}
#endif

inline unsigned int pkt_datalen(struct pkt* p)
{
    return (p->hdr.data_end - p->hdr.data);
}

inline uint8_t* pkt_mac_header(struct pkt* p)
{
    return p->hdr.mac_header;
}

inline uint8_t* pkt_network_header(struct pkt* p)
{
    return p->hdr.network_header;
}

inline uint8_t* pkt_transport_header(struct pkt* p)
{
    return p->hdr.transport_header;
}

